/* 
 * File:   Wireless Multimeter Firmware
 * Author: Brennan Ujcich
 *
 * Created on October 4, 2013
 */

#include <p18f45k22.h>
#include <p18cxxx.h>
#include <delays.h>
#include <stdlib.h>
#include <usart.h>
#include <string.h>

#pragma config FOSC = INTIO67, PLLCFG = OFF, PRICLKEN = ON, FCMEN = ON, PWRTEN = OFF
#pragma config BOREN = SBORDIS, BORV = 250, WDTEN = OFF, WDTPS = 4096, PBADEN = OFF
#pragma config HFOFST = OFF, MCLRE = EXTMCLR, STVREN = ON, LVP = OFF, DEBUG = ON, CCP2MX = PORTB3

//RedShark Version 1 Constant
#define REDSHARK_V1         0b01000001

//ACK Constant
#define ACK_OK               0x00
#define ACK_INVALID_COMMAND  0x01
#define ACK_BAD_CHECKSUM     0x02
#define ACK_INCOMPLETE_MSG   0x03
#define ACK_BEGIN_DATA       0x04
#define ACK_END_DATA         0x05

#define ACK_INVALID_CFG      0x64

unsigned char lastMsgNum;

//USART Buffers and stuff
unsigned char usartChar;
unsigned char receiveBuffer[50]; //Only need 5 but want room for overflow
unsigned char processBuffer[50];

//Iterator for clearing arrays
unsigned char mainCopyIter;

//Count of characters currently in buffer
unsigned char count = 0;

//Last messageNum for the scope
//  This will be a 2 bit number
unsigned char scopeMsgNum;

//Scope channel
unsigned char scopeChannel;
unsigned char scopeEnabled;

unsigned long triggerTimeout;
unsigned int scopeTemp1;
unsigned int scopeTemp2;
unsigned int scopeTrigger;
unsigned int scopeMax;
unsigned int scopeMin;

//Scope 
unsigned int scopeIter;

//Frequency
unsigned int freqTimeoutCounter;
unsigned char freqTimedOut;

//Resistance
unsigned short long averageTotal;
unsigned char averageIter;

unsigned char messageNum;
unsigned char data[50];
#define ACK_COUNTER_OVERFLOW 0x06
#define ACK_COUNTER_EDGE_NOT_FOUND 0x07;

#define CFG_FREQ_LOW 0x00
#define CFG_FREQ_HIGH 0x01

#define RESISTANCE_DELAY 255
#define NUM_AVERAGE_POINTS 64

//See this URL for why we had to use these wierd pragmas
//  http://www.xargs.com/pic/c18large.html
#pragma udata scope_udata
unsigned int scopeBuffer[256];
#pragma udata
unsigned int *scopeData;

void high_ISR(void);
#pragma code high_vector = 0x08	// force the following statement to start at

void high_interrupt(void) // 0x08
{
    _asm
    goto high_ISR
            _endasm
}
#pragma code 			//return to the default code section
#pragma interrupt high_ISR

void high_ISR(void) {
    usartChar = getc1USART();
    if (usartChar == '[') {
        count = 0;
    }
    receiveBuffer[count] = usartChar;
    count++;
    return;
}

/*
 *
 * This Function is an override for puts1USART. It takes a number of
 * bytes as a paramteter instead of going until it hits a null character.
 *
 * @param data Data to transmit
 * @param numBytes number of bytes to send
 *
 *
 */
void putb1USART(char *data, unsigned char numBytes) {
    unsigned char i;
    for (i = 0; i < numBytes; i++) {
        // Transmit a byte
        while (Busy1USART());
        putc1USART(*data);
        *data++;
    }
}

int SendResponse(unsigned char command, unsigned char messageNum, unsigned char length, unsigned char* data) {
    unsigned char i;
    unsigned char response[50];
    unsigned char numDataBytes;

    //Check if we might need to allow for length to be 0, but have an ACK code
    if (length == 0) {
        numDataBytes = 1;
    } else {
        numDataBytes = length;
    }

    //Set beginning and end chars
    response[0] = '[';
    response[4 + numDataBytes] = ']';

    //Set the response bit, and combine with command.
    command = command & 0b01111111;
    response[1] = 0b10000000;
    response[1] = response[1] | command;

    //Shift the messageNum over and specify the length
    response[2] = messageNum & 0b00000111;
    response[2] = response[2] << 5;
    response[2] = response[2] | length;

    //Fill Data /nt
    for (i = 0; i < numDataBytes; i++) {
        response[3 + i] = data[i];
    }

    //Calc XOR Checksum
    response[3 + numDataBytes] = response[1] ^ response[2];
    for (i = 3; i < (numDataBytes + 3); i++) {
        response[3 + numDataBytes] = response[3 + numDataBytes] ^ response[i];
    }
    //response[5 + numDataBytes] = 0x00;
    putb1USART(response, 5 + numDataBytes);
}

int SendScopeResponse(unsigned int data) {
    unsigned char firstByte;
    unsigned char secondByte;

    //Grab the top five data bits and shift them right
    firstByte = (data & 0b0000001111100000) >> 5;
    firstByte = firstByte | (scopeMsgNum << 5); //Throw on a two digit

    secondByte = (data & 0b0000000000011111); //Grab the bottom 5
    secondByte = secondByte | (scopeMsgNum << 5); //Tag on the same message num
    secondByte = secondByte | 0b10000000; //Tag the first bit as 1 to designate its the second packet

    scopeMsgNum = (scopeMsgNum + 1) % 5;

    while (Busy1USART());
    putc1USART(firstByte);

    while (Busy1USART());
    putc1USART(secondByte);
}

unsigned int readADC(unsigned char channel) {
    ADCON0 = (channel << 2) | 0b01;
    ADCON0bits.GO = 1;
    while (ADCON0bits.GO);
    return ADRES;
}

unsigned void setMaxMinTrigger(unsigned char channel, unsigned char delay) {
    scopeMax = 0;
    scopeMin = 1024;
    for (scopeIter = 0; scopeIter < 256; scopeIter++) {
        scopeTemp1 = readADC(channel);
        if (scopeTemp1 > scopeMax) {
            scopeMax = scopeTemp1;
        }
        if (scopeTemp1 < scopeMin) {
            scopeMin = scopeTemp1;
        }
        Delay10TCYx(delay);
    }

    scopeTrigger = ((scopeMax - scopeMin) >> 1) + scopeMin;
}

void doScope(unsigned char command, unsigned char delay) {
    //Set the channel from the command
    unsigned int channel;
    switch (command) {
        case 71:
            channel = 0;
            break;
        case 72:
            channel = 1;
            break;
        case 73:
            channel = 2;
            break;
        case 74:
            channel = 20;
            break;
        case 75:
            channel = 21;
            break;
        case 76:
            channel = 22;
            break;
        case 77:
            channel = 23;
            break;
    }

    setMaxMinTrigger(channel, delay);
    scopeTemp1 = scopeTrigger;
    scopeTemp2 = readADC(channel);

    triggerTimeout = 0;

    scopeIter = 0;
    while (scopeIter < 3 && triggerTimeout < 768) {
        //Check and if we did not increase, restart the for loop
        if (scopeTemp2 < (scopeTemp1 - 10)) {
            scopeIter = 0;

            // Also, reset the Trigger.
            scopeTemp1 = scopeTrigger;

            triggerTimeout++;
        } else { //Otherwise, copy the new value to scopeTemp1
            scopeTemp1 = scopeTemp2;
        }

        //Get a new data point to compare
        scopeTemp2 = readADC(channel);

        //Finally, whenever we hit 5 increasing points in a row, we will break.
        scopeIter++;

    }

    if (triggerTimeout > 100) {
        triggerTimeout = 0;
    }

    //Read in da data.
    for (scopeIter = 0; scopeIter < 256; scopeIter++) {
        scopeData[scopeIter] = readADC(channel);
        Delay10TCYx(delay);
    }

    //Send preface
    data[0] = ACK_BEGIN_DATA;
    SendResponse(command, messageNum, 0, data);

    //Send da data
    for (scopeIter = 0; scopeIter < 256; scopeIter++) {
        SendScopeResponse(scopeData[scopeIter]);
    }

    //Tell 'em were done
    data[0] = ACK_END_DATA;
    SendResponse(command, messageNum, 0, data);

}

int RecieveCommand(char request[5]) {
    //Recieve Vars
    unsigned char type;

    unsigned char checkedSum;
    unsigned char optionalCfg;

    unsigned char dataPos = 0;

    unsigned int adResult;

    //Check to see if properly formatted.
    if (request[0] == '[' && request[4] == ']') {

        //Check checksum
        checkedSum = request[1] ^ request[2];
        if (request[3] != checkedSum) {
            return 1;
        }

        //Grab first bit, mask it and shift it to the right.
        type = request[1] & 0b10000000;
        type = type >> 7;
        if (type != 0) {
            data[0] = ACK_BAD_CHECKSUM;
            SendResponse(request[1], messageNum, 0, data);
            return 1;
        }

        //Check Message Number
        messageNum = request[2] & 0b11100000;
        messageNum = messageNum >> 5;
        if (messageNum == lastMsgNum && lastMsgNum != 255) {
            return 1;
        }
        lastMsgNum = messageNum;

        //Optional Config
        optionalCfg = request[2] & 0b00011111;

        switch (request[1]) {
            case 0:
                //Send Init
                data[0] = 0x01; //Protocol Version
                data[1] = REDSHARK_V1; //Company Version
                SendResponse(request[1], messageNum, 2, data);
                break;
            case 95:
                //Standard A/D Settings
                ADCON2bits.ACQT = 0b111; //20TAD for best accuracy
                ADCON2bits.ADCS = 0b111; //FRC

                //Debug Case
                adResult = readADC(20);
                data[dataPos++] = (adResult & 0xFF00) >> 8;
                data[dataPos++] = adResult & 0x00FF;

                adResult = readADC(21);
                data[dataPos++] = (adResult & 0xFF00) >> 8;
                data[dataPos++] = adResult & 0x00FF;

                SendResponse(request[1], messageNum, 4, data); //
                break;
            case 64:
                //Standard A/D Settings
                ADCON2bits.ACQT = 0b111; //20TAD for best accuracy
                ADCON2bits.ADCS = 0b111; //FRC

                //Send DC Voltage
                adResult = readADC(0);
                data[dataPos++] = (adResult & 0xFF00) >> 8;
                data[dataPos++] = adResult & 0x00FF;

                adResult = readADC(1);
                data[dataPos++] = (adResult & 0xFF00) >> 8;
                data[dataPos++] = adResult & 0x00FF;

                adResult = readADC(2);
                data[dataPos++] = (adResult & 0xFF00) >> 8;
                data[dataPos++] = adResult & 0x00FF;

                adResult = readADC(20);
                data[dataPos++] = (adResult & 0xFF00) >> 8;
                data[dataPos++] = adResult & 0x00FF;

                adResult = readADC(21);
                data[dataPos++] = (adResult & 0xFF00) >> 8;
                data[dataPos++] = adResult & 0x00FF;

                SendResponse(request[1], messageNum, 10, data);
                break;
            case 65:
                //Standard A/D Settings
                ADCON2bits.ACQT = 0b111; //20TAD for best accuracy
                ADCON2bits.ADCS = 0b111; //FRC

                //Send Current
                averageTotal = 0;
                for (averageIter = 0; averageIter < NUM_AVERAGE_POINTS; averageIter++) {
                    averageTotal += readADC(22);
                }
                averageTotal >>= 6; //Divide by 64
                data[dataPos++] = (averageTotal & 0xFF00) >> 8;
                data[dataPos++] = averageTotal & 0x00FF;

                averageTotal = 0;
                for (averageIter = 0; averageIter < NUM_AVERAGE_POINTS; averageIter++) {
                    averageTotal += readADC(23);
                }
                averageTotal >>= 6; //Divide by 64
                data[dataPos++] = (averageTotal & 0xFF00) >> 8;
                data[dataPos++] = averageTotal & 0x00FF;

                SendResponse(request[1], messageNum, 4, data);
                break;
            case 66:
                //Standard A/D Settings
                ADCON2bits.ACQT = 0b111; //20TAD for best accuracy
                ADCON2bits.ADCS = 0b111; //FRC

                //Resistance
                LATDbits.LATD7 = 0;
                LATDbits.LATD6 = 0;
                LATDbits.LATD5 = 0;

                Delay10KTCYx(RESISTANCE_DELAY);
                averageTotal = 0;
                for (averageIter = 0; averageIter < NUM_AVERAGE_POINTS; averageIter++) {
                    averageTotal += readADC(24);
                }
                averageTotal >>= 6; //Divide by 64

                data[dataPos++] = (averageTotal & 0xFF00) >> 8;
                data[dataPos++] = averageTotal & 0x00FF;

                LATDbits.LATD7 = 0;
                LATDbits.LATD6 = 0;
                LATDbits.LATD5 = 1;

                Delay10KTCYx(RESISTANCE_DELAY);
                averageTotal = 0;
                for (averageIter = 0; averageIter < NUM_AVERAGE_POINTS; averageIter++) {
                    averageTotal += readADC(24);
                }
                averageTotal >>= 6; //Divide by 64

                data[dataPos++] = (averageTotal & 0xFF00) >> 8;
                data[dataPos++] = averageTotal & 0x00FF;

                LATDbits.LATD7 = 0;
                LATDbits.LATD6 = 1;
                LATDbits.LATD5 = 0;

                Delay10KTCYx(RESISTANCE_DELAY);
                averageTotal = 0;
                for (averageIter = 0; averageIter < NUM_AVERAGE_POINTS; averageIter++) {
                    averageTotal += readADC(24);
                }
                averageTotal >>= 6; //Divide by 64

                data[dataPos++] = (averageTotal & 0xFF00) >> 8;
                data[dataPos++] = averageTotal & 0x00FF;

                LATDbits.LATD7 = 0;
                LATDbits.LATD6 = 1;
                LATDbits.LATD5 = 1;

                Delay10KTCYx(RESISTANCE_DELAY);
                averageTotal = 0;
                for (averageIter = 0; averageIter < NUM_AVERAGE_POINTS; averageIter++) {
                    averageTotal += readADC(24);
                }
                averageTotal >>= 6; //Divide by 64

                data[dataPos++] = (averageTotal & 0xFF00) >> 8;
                data[dataPos++] = averageTotal & 0x00FF;

                LATDbits.LATD7 = 1;
                LATDbits.LATD6 = 0;
                LATDbits.LATD5 = 0;

                Delay10KTCYx(RESISTANCE_DELAY);
                averageTotal = 0;
                for (averageIter = 0; averageIter < NUM_AVERAGE_POINTS; averageIter++) {
                    averageTotal += readADC(24);
                }
                averageTotal >>= 6; //Divide by 64

                data[dataPos++] = (averageTotal & 0xFF00) >> 8;
                data[dataPos++] = averageTotal & 0x00FF;

                LATDbits.LATD7 = 1;
                LATDbits.LATD6 = 0;
                LATDbits.LATD5 = 1;

                Delay10KTCYx(RESISTANCE_DELAY);
                averageTotal = 0;
                for (averageIter = 0; averageIter < NUM_AVERAGE_POINTS; averageIter++) {
                    averageTotal += readADC(24);
                }
                averageTotal >>= 6; //Divide by 64

                data[dataPos++] = (averageTotal & 0xFF00) >> 8;
                data[dataPos++] = averageTotal & 0x00FF;

                LATDbits.LATD7 = 1;
                LATDbits.LATD6 = 1;
                LATDbits.LATD5 = 0;

                Delay10KTCYx(RESISTANCE_DELAY);
                averageTotal = 0;
                for (averageIter = 0; averageIter < NUM_AVERAGE_POINTS; averageIter++) {
                    averageTotal += readADC(24);
                }
                averageTotal >>= 6; //Divide by 64

                data[dataPos++] = (averageTotal & 0xFF00) >> 8;
                data[dataPos++] = averageTotal & 0x00FF;

                LATDbits.LATD7 = 1;
                LATDbits.LATD6 = 1;
                LATDbits.LATD5 = 1;

                Delay10KTCYx(RESISTANCE_DELAY);
                averageTotal = 0;
                for (averageIter = 0; averageIter < NUM_AVERAGE_POINTS; averageIter++) {
                    averageTotal += readADC(24);
                }
                averageTotal >>= 6; //Divide by 64

                data[dataPos++] = (averageTotal & 0xFF00) >> 8;
                data[dataPos++] = averageTotal & 0x00FF;

                SendResponse(request[1], messageNum, 16, data);
                break;
            case 67:
                TMR1H = 0x00;
                TMR1L = 0x00;

                T1GCONbits.T1GVAL = 0;
                T1GCONbits.T1GGO = 1;

                //Reset our flags/couters
                freqTimeoutCounter = 0;
                freqTimedOut = 0;
                PIR1bits.TMR1IF = 0; //Reset Overflow

                //Get Config
                //Determine if 0 then 1:1 if 1 then 1:32
                if (optionalCfg == CFG_FREQ_LOW) {
                    T1CON = 0b00110011;
                } else if (optionalCfg == CFG_FREQ_HIGH) {
                    T1CON = 0b01000011;
                } else {
                    data[0] = ACK_INVALID_CFG;
                    SendResponse(request[1], messageNum, 0, data);
                }

                //Wait till not done or timeout
                while (T1GCONbits.T1GGO_NOT_DONE && freqTimeoutCounter < 0xFFFF) {
                    Delay10TCYx(2);
                    freqTimeoutCounter++;

                    //If our counter gets above this value ~.1s has
                    //elapsed and we should break out and give an error
                    if (freqTimeoutCounter >= 0xFFFF) {
                        freqTimedOut = 1;
                        break;
                    }
                }
                if (PIR1bits.TMR1IF == 1) {
                    data[0] = ACK_COUNTER_OVERFLOW;

                    SendResponse(request[1], messageNum, 0, data);
                } else if (freqTimedOut == 1) {
                    data[0] = ACK_COUNTER_EDGE_NOT_FOUND;
                    SendResponse(request[1], messageNum, 0, data);
                } else { //Normal Case
                    data[1] = TMR1L; //Must send TMR1L first since in 16bit RW mode. see 12.6 in DataSheet
                    data[0] = TMR1H;

                    SendResponse(request[1], messageNum, 2, data);
                }
                break;
            case 68:
                LATDbits.LATD7 = 0;
                LATDbits.LATD6 = 0;
                LATDbits.LATD5 = 1;

                Delay10KTCYx(160);
                adResult = readADC(24);
                data[dataPos++] = (adResult & 0xFF00) >> 8;
                data[dataPos++] = adResult & 0x00FF;

                SendResponse(request[1], messageNum, 2, data);
                break;
            case 69:
                //Standard A/D Settings
                ADCON2bits.ACQT = 0b111; //20TAD for best accuracy
                ADCON2bits.ADCS = 0b111; //FRC

                //Resistance
                LATDbits.LATD7 = 0;
                LATDbits.LATD6 = 0;
                LATDbits.LATD5 = 0;

                Delay10KTCYx(RESISTANCE_DELAY);
                averageTotal = 0;
                for (averageIter = 0; averageIter < 64; averageIter++) {
                    averageTotal += readADC(24);
                }
                averageTotal >> 6; //Divide by 64

                data[dataPos++] = (averageTotal & 0xFF00) >> 8;
                data[dataPos++] = averageTotal & 0x00FF;
                break;
            case 71:
            case 72:
            case 73:
            case 74:
            case 75:
            case 76:
            case 77://76, 77 are for Current sensors
                //Scope Settings
                ADCON2bits.ACQT = 0b001; //TODO: Least amount of TAD
                ADCON2bits.ADCS = 0b110; //This TAD is technically too fast, but I dont care.

                //Call 'da method and the function will do the rest
                doScope(request[1], optionalCfg);
                break;
            case 80:
                //Standard A/D Settings
                ADCON2bits.ACQT = 0b111; //20TAD for best accuracy
                ADCON2bits.ADCS = 0b111; //FRC

                //Battery Check
                adResult = readADC(15);
                data[dataPos++] = (adResult & 0xFF00) >> 8;
                data[dataPos++] = adResult & 0x00FF;

                SendResponse(request[1], messageNum, 2, data); //
                break;
            default:
                //Send Init
                data[0] = ACK_INVALID_COMMAND; //Send Back OK
                SendResponse(request[1], messageNum, 0, data);
                break;
                return;
        }
    }
}

void main(void) {
    //configure the Oscillator for 64Mhz internal oscillator operation
    OSCCON2 = 0b10000000; //4X PLL
    OSCCON = 0b01110000; // 16MHZ
    OSCTUNE = 0b11000000;

    //USART SETUP
    ANSELC = 0b01000000;
    TRISC = 0b10000000;

    //AD Setup
    ADCON1 = 0b00000100; //A/D VREF+ connected to external pin, VREF- connected to internal
    ADCON2 = 0b10011111;

    //Standard A/D Settings
    ADCON2bits.ACQT = 0b111; //20TAD for best accuracy
    ADCON2bits.ADCS = 0b111; //FRC

    //Settings for using external voltage reference
    // TRISAbits.TRISA3 = 1;
    //ANSELAbits.ANSA3 = 1;

    //AD Port Setup
    ANSELA = 0b00001111;
    ANSELD = 0b11111111;
    TRISA = 0xFF;
    TRISD = 0b00011111;

    ANSELCbits.ANSC3 = 1;
    TRISCbits.TRISC3 = 1;

    //Counter AD Port
    ANSELBbits.ANSB5 = 0;
    TRISBbits.TRISB5 = 1;

    //Counter Setup
    T0CON = 0b10101010;

    //Frequency Counter Setup
    //  T1CON setup in the frequency response
    //  So that we can slect the clock value
    T1GCON = 0b11110000;

    //Debug LED
    TRISBbits.RB0 = 0;
    ANSELBbits.ANSB0 = 1;
    PORTBbits.RB0 = 0;

    Open1USART(USART_TX_INT_OFF &
            USART_RX_INT_ON &
            USART_ASYNCH_MODE &
            USART_EIGHT_BIT &
            USART_CONT_RX &
            USART_BRGH_HIGH,
            34);
    INTCONbits.PEIE_GIEL = 1;
    INTCONbits.GIE_GIEH = 1;

    scopeMsgNum = 0;
    lastMsgNum = 255;
    scopeEnabled = 0;

    //Setup Scope Data Pointer
    //Note: I had to use a workaround
    //  http://www.xargs.com/pic/c18large.html
    scopeData = scopeBuffer;

    for (mainCopyIter = 0; mainCopyIter < 50; mainCopyIter++) {
        processBuffer[mainCopyIter] = '*';
        receiveBuffer[mainCopyIter] = '*';
    }

    //DEBUG

    LATDbits.LATD7 = 0; //MSB
    LATDbits.LATD6 = 0;
    LATDbits.LATD5 = 0; //LSB

    //Debug to output count
    TRISBbits.RB0 = 0;
    TRISBbits.RB1 = 0;
    TRISBbits.RB2 = 0;
    TRISBbits.RB3 = 0;
    TRISBbits.RB4 = 0;


    ANSELBbits.ANSB0 = 1;
    ANSELBbits.ANSB1 = 1;
    ANSELBbits.ANSB2 = 1;
    ANSELBbits.ANSB3 = 1;
    ANSELBbits.ANSB4 = 1;

    while (1) {

        if (count == 5) {
            processBuffer[0] = receiveBuffer[0];
            processBuffer[1] = receiveBuffer[1];
            processBuffer[2] = receiveBuffer[2];
            processBuffer[3] = receiveBuffer[3];
            processBuffer[4] = receiveBuffer[4];
            RecieveCommand(processBuffer);
        }
        if (scopeEnabled == 1) {
        }

    }

}